#include <iostream>

#include "operations.h"
#include "bit-field.h"
#include "integral.h"

CoreOperation::OP CoreOperation::op_from_string (const string& str)
{
  string tmp = str;
  string_to_lower (tmp);

  if (tmp == "adcadd")
    return CoreOperation::ADCADD;
  else if (tmp == "adiw")
    return CoreOperation::ADIW;
  else if (tmp == "and")
    return CoreOperation::AND;
  else if (tmp == "andi")
    return CoreOperation::ANDI;
  else if (tmp == "asr")
    return CoreOperation::ASR;
  else if (tmp == "bldbst")
    return CoreOperation::BLDBST;
  else if (tmp == "brbcbrbs")
    return CoreOperation::BRBCBRBS;
  else if (tmp == "bsetclr")
    return CoreOperation::BSETCLR;
  else if (tmp == "call")
    return CoreOperation::CALL;
  else if (tmp == "cbisbi")
    return CoreOperation::CBISBI;
  else if (tmp == "com")
    return CoreOperation::COM;
  else if (tmp == "cpcpc")
    return CoreOperation::CPCPC;
  else if (tmp == "cpi")
    return CoreOperation::CPI;
  else if (tmp == "ori")
    return CoreOperation::ORI;

  return CoreOperation::INVALID;
}

void CoreOperation::string_to_lower (string& str)
{
  string::iterator beg = str.begin ();
  string::iterator end = str.end ();

  for (; beg != end; ++beg) {
    if (*beg <= 'Z')
      *beg += 32;
  }
}



int ADCADDOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);
  UInt16 r = unpack_mask<uint16_t> (instruction, 0x020f);
  bool use_carry = get_bit<uint16_t> (instruction, 12);

  UInt8 Rd = core.read (d);
  UInt8 Rr = core.read (r);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = Rd + Rr;

  if (use_carry) {
    UInt8 c = get_bit<uint8_t> (SREG, C_FLAG);
    R += c;
  }

  core.write (d, R);

  SREG[H_FLAG] = (Rd[3] & Rr[3]) | (Rr[3] & !R[3]) | (!R[3] & Rd[3]);
  SREG[V_FLAG] = (Rd[7] & Rr[7] & !R[7]) | (!Rd[7] & !Rr[7] & R[7]);
  SREG[C_FLAG] = (Rd[7] & !R[7]) | (Rr[7] & !R[7]) | (!R[7] & Rd[7]);
  SREG[Z_FLAG] = (R == 0);
  SREG[N_FLAG] = R[7];
  SREG[S_FLAG] = SREG[N_FLAG] ^ SREG[V_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int ADIWOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x0030) * 2 + 24;
  UInt16 k = unpack_mask<uint16_t> (instruction, 0x00cf);

  UInt16 Rp;
  Rp.set_byte (0, core.read (d));
  Rp.set_byte (1, core.read (d + 1));
  UInt8 SREG = core.get_sreg ();

  UInt16 R = Rp + k;

  core.write (d, R.get_byte (0));
  core.write (d + 1, R.get_byte (1));

  SREG[Z_FLAG] = (R == 0);
  SREG[V_FLAG] = !Rp[15] & R[15];
  SREG[N_FLAG] = R[15];
  SREG[S_FLAG] = SREG[N_FLAG] ^ SREG[V_FLAG];
  SREG[C_FLAG] = R[15] & Rp[15];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 2;
}



int ANDOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);
  UInt16 r = unpack_mask<uint16_t> (instruction, 0x020f);

  UInt8 Rd = core.read (d);
  UInt8 Rr = core.read (r);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = Rd & Rr;

  core.write (d, R);

  SREG[V_FLAG] = 0;
  SREG[N_FLAG] = R[7];
  SREG[Z_FLAG] = (R == 0);
  SREG[S_FLAG] = SREG[N_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int ANDIOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 reg = unpack_mask<uint16_t> (instruction, 0x00f0) + 16;
  UInt8 imm = unpack_mask<uint16_t> (instruction, 0x0f0f);

  UInt8 Rd = core.read (reg);
  UInt8 SREG = core.get_sreg ();

  UInt8 Rr = Rd & imm;

  core.write (reg, Rr);

  SREG[Z_FLAG] = (Rr == 0);
  SREG[N_FLAG] = Rr[7];
  SREG[V_FLAG] = 0;
  SREG[S_FLAG] = SREG[N_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int ASROperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);

  UInt8 Rd = core.read (d);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = Rd >> 1;
  R[7] = R[6];

  core.write (d, R);

  SREG[C_FLAG] = Rd[0];
  SREG[Z_FLAG] = (R == 0);
  SREG[N_FLAG] = R[7];
  SREG[V_FLAG] = SREG[N_FLAG] ^ SREG[C_FLAG];
  SREG[S_FLAG] = SREG[N_FLAG] ^ SREG[V_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int BSETCLROperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt8 s = unpack_mask<uint16_t> (instruction, 0x0070);
  bool clr = get_bit<uint16_t> (instruction, 7);

  UInt8 SREG = core.get_sreg ();

  if (clr)
    SREG[s] = 0;
  else
    SREG[s] = 1;

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int BLDBSTOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);
  UInt16 b = unpack_mask<uint16_t> (instruction, 0x0007);
  bool store = get_bit<uint16_t> (instruction, 9);

  UInt8 Rd = core.read (d);
  UInt8 SREG = core.get_sreg ();

  if (store) {
    SREG[T_FLAG] = Rd[b];

    core.set_sreg (SREG);
  }
  else {
    Rd[b] = SREG[T_FLAG];

    core.write (d, Rd);
  }

  core.inc_pc (1);

  return 1;
}



int BRBCBRBSOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 k = unpack_mask<uint16_t> (instruction, 0x03f8);
  UInt16 s = unpack_mask<uint16_t> (instruction, 0x0007);
  bool branch_if_clear = get_bit<uint16_t> (instruction, 10);

  UInt8 SREG = core.get_sreg ();

  if ((bool)SREG[s] != branch_if_clear) {
    /* Sign extend k */
    for (int i = 7; i < 16; ++i)
      k[i] = k[7];

    core.inc_pc (k + 1);

    return 2;
  }

  core.inc_pc (1);

  return 1;
}



int CALLOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 return_pc = core.get_pc () + 2;
  UInt16 sp = core.get_sp ();

  core.write (sp--, return_pc.get_byte (0));
  core.write (sp--, return_pc.get_byte (1));

  core.set_pc (core.get_flash ()[return_pc - 1]);

  return 4;
}



int CBISBIOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 A = unpack_mask<uint16_t> (instruction, 0x00f8) + 32;
  UInt8 b = unpack_mask<uint8_t> (instruction, 0x0007);
  bool bit = get_bit<uint16_t> (instruction, 9);

  UInt8 io = core.read (A);

  io[b] = bit;

  core.write (A, io);

  core.inc_pc (1);

  return 2;
}



int COMOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);

  UInt8 Rd = core.read (d);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = 0xff - Rd;

  SREG[V_FLAG] = 0;
  SREG[N_FLAG] = R[7];
  SREG[Z_FLAG] = (R == 0);
  SREG[C_FLAG] = 1;
  SREG[S_FLAG] = SREG[N_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int CPCPCOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x01f0);
  UInt16 r = unpack_mask<uint16_t> (instruction, 0x020f);
  bool use_carry = get_bit<uint16_t> (instruction, 12);

  UInt8 Rd = core.read (d);
  UInt8 Rr = core.read (r);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = Rd - Rr;
  if (use_carry)
    R -= SREG[C_FLAG];

  SREG[H_FLAG] = (!Rd[3] & Rr[3]) | (Rr[3] & R[3]) | (R[3] & !Rd[3]);
  SREG[V_FLAG] = (Rd[7] & !Rr[7] & !R[7]) | (!Rd[7] & Rr[7] & R[7]);
  SREG[N_FLAG] = R[7];
  SREG[Z_FLAG] = (R == 0);
  SREG[C_FLAG] = (!Rd[7] & Rr[7]) | (Rr[7] & R[7]) | (R[7] & !Rd[7]);
  SREG[S_FLAG] = SREG[N_FLAG] ^ SREG[V_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int CPIOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt16 d = unpack_mask<uint16_t> (instruction, 0x00f0);
  UInt8 k = unpack_mask<uint16_t> (instruction, 0x0f0f);

  UInt8 Rd = core.read (d);
  UInt8 SREG = core.get_sreg ();

  UInt8 R = Rd - k;

  SREG[H_FLAG] = (!Rd[3] & k[3]) | (k[3] & R[3]) | (R[3] & !Rd[3]);
  SREG[V_FLAG] = (Rd[7] & !k[7] & !R[7]) | (!Rd[7] & k[7] & R[7]);
  SREG[N_FLAG] = R[7];
  SREG[Z_FLAG] = (R == 0);
  SREG[C_FLAG] = (!Rd[7] & k[7]) | (k[7] & R[7]) | (R[7] & !Rd[7]);
  SREG[S_FLAG] = SREG[N_FLAG] ^ SREG[V_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}



int CPSEOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt8 d = unpack_mask<uint16_t> (instruction, 0x01f0);
  UInt8 r = unpack_mask<uint16_t> (instruction, 0x020f);

  UInt8 Rd = core.read (d);
  UInt8 Rr = core.read (r);

  if (Rd == Rr) {
    core.inc_pc (2);
    return 2;
  }

  core.inc_pc (1);

  return 1;
}



int ORIOperation::execute (uint16_t instruction, AVRCore& core)
{
  UInt8 d = unpack_mask<uint16_t> (instruction, 0x00f0) + 16;
  UInt8 k = unpack_mask<uint16_t> (instruction, 0x0f0f);

  UInt8 Rd = core.read (d);
  UInt8 R = Rd | k;
  UInt8 SREG = core.get_sreg ();

  core.write (d, R);

  SREG[Z_FLAG] = (R == 0);
  SREG[N_FLAG] = R[7];
  SREG[V_FLAG] = 0;
  SREG[S_FLAG] = SREG[N_FLAG];

  core.set_sreg (SREG);
  core.inc_pc (1);

  return 1;
}
